%作业6_8

%% 准备工作
motor_res = zeros(n_group,length(1:simu_step),3);
state_info = zeros(n_group,length(1:simu_step),3);
debug_info = zeros(n_group,length(1:simu_step),4);

a_sj.reset_state(repmat(initial_state,2,1));

%ts = 1, zeta = 1
is_int_enable = [0;1];
inv_dynamic_con = newPidControler(a_sj,tor_motor,[0.1;0.1],[1;1],is_int_enable);
%先把pid架设好
con_value = 0;
sample_time_count = sample_period;

%计算理论的稳态值
% equ = @(th)(th-(a_sj.m*a_sj.g*a_sj.l - a_sj.f_m*a_sj.g*a_sj.f_l)*sin(th)...
%     /(a_sj.n_factor.*a_sj.f_Im.*inv_dynamic_con.pid_kp));
% 
% th_ess = fsolve(equ,[5*pi/180;5*pi/180])


%% loop

for i = 1:simu_step
    if(sample_time_count >= sample_period)
        inv_dynamic_con.set_target([step_value 0 0]);
        debug_info(:,i,:) =  inv_dynamic_con.get_id_con_value(a_sj,tor_motor,sample_period);
        con_value = debug_info(:,i,1);
        sample_time_count = sample_time_count-sample_period;
    else
        debug_info(:,i,:) = debug_info(:,i-1,:);
    end
    motor_res(:,i,:) = tor_motor.simu_loop(a_sj,simu_period,con_value);
    state_info(:,i,:) = (a_sj.simu_loop(motor_res(:,i,1),simu_period));
    sample_time_count = sample_time_count+simu_period;
end


%%



%绘图
figure;
plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,1),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
hold on
yline(step_value,'r--');
legend('no int','using int','target')
title("pos hold  theta");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,2),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% yline(0,'r--');
% legend('no int','using int','target')
% title("pos hold  omega");
% figure;
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(1,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% plot(0:simu_period:(simu_step-1)*simu_period,state_info(2,:,3),'-*','MarkerIndices',1:8000:simu_step,'MarkerSize',2);
% hold on
% yline(0,'r--');
% legend('no int','using int','target')
% title("pos hold  acc");